#include "mjbot_spi_interface.hpp"
Mjbot_SPI_Interface::Mjbot_SPI_Interface(const char* h_spi, int freq, int delay, int bits_per_word, int mode)
{
    this->h_spi = h_spi;
    this->spi_freq = freq;
    this->delay = delay;
    this->bits_per_word = bits_per_word;
    this->mode = mode;
}

int Mjbot_SPI_Interface::send(uint8_t dev, uint16_t id, uint8_t *pdata, uint8_t length)
{
    int ret;
    //发送数据
    char tx_buf[64] = {};
    char rx_buf[64] = {};

    // printf("cmd length:%d\n", length);
    this->spi_fd = open(this->h_spi, O_RDWR);
    if (spi_fd < 0) {
        perror("Error: Cann't open SPI Dev.\n");
        return -1;
    }

    //配置SPI参数
    ret = ioctl(spi_fd, SPI_IOC_WR_MODE, &this->mode);
    if (ret == -1) {
        perror("Error: SPI_IOC_WR_MODE fault.\n");
        return -1;
    }

    ret = ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &this->bits_per_word);
    if (ret == -1) {
        perror("Error: SPI_IOC_WR_BITS fault.\n");
        return -1;
    }

    ret = ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &this->spi_freq);
    if (ret == -1) {
        perror("Error: SPI_IOC_WR_MAX_SPEED fault.\n");
        return -1;
    }

    struct spi_ioc_transfer spi { };
    dev |= 0x80;
    spi.tx_buf = (unsigned long)&dev;
    spi.rx_buf = (unsigned long)rx_buf;
    spi.len = 1;
    spi.delay_usecs = this->delay;
    spi.speed_hz = this->spi_freq;
    spi.bits_per_word = this->bits_per_word;
    // Send WR_ADDR
    ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
    if (ret < 1) {
        perror("Error: SPI_IOC_MESSAGE fault.\n");
        return -1;
    }
    else {
        // printf("addr recv: 0x%x\n", rx_buf[0]);
    }


    spi.tx_buf = (unsigned long)&id;
    spi.rx_buf = (unsigned long)rx_buf;
    spi.len = 2;
    // Send CANID
    ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
    if (ret < 1) 
    {
        perror("Error");
    }
    else
    {
        // printf("id_rcv: 0x%x\n", rx_buf[0]);
    }

    // printf("transmit suc!\n");
    spi.tx_buf = (unsigned long)&length;
    spi.rx_buf = (unsigned long)rx_buf;
    spi.len = 1;
    // Send length
    ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
    if (ret < 1)
    {
        perror("Error");
    }
    else
    {
        // printf("len_recv: 0x%x\n", rx_buf[0]);
    }

    // printf("transmit suc!\n");

    memcpy(tx_buf, pdata, length);
    spi.tx_buf = (unsigned long)tx_buf;
    spi.rx_buf = (unsigned long)rx_buf;
    spi.len = length;
    // Send message
    ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
    if (ret < 1)
    {
        perror("Error");
    }
    else
    {
        // printf("data_recv: 0x%x\n", rx_buf[0]);
    }

    close(spi_fd);
    return 0;
}

int Mjbot_SPI_Interface::receive(uint8_t dev, uint16_t *id, uint8_t* pdata, uint8_t *length)
{
    int ret = 0;
    //发送数据
    char tx_buf[64] = {};
    char rx_buf[64] = {};
    this->spi_fd = open(this->h_spi, O_RDWR);
    if (spi_fd < 0) {
        perror("Error: Cann't open SPI Dev.\n");
        return -1;
    }

    //配置SPI参数
    ret = ioctl(spi_fd, SPI_IOC_WR_MODE, &this->mode);
    if (ret == -1) {
        perror("Error: SPI_IOC_WR_MODE fault.\n");
        return -1;
    }

    ret = ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &this->bits_per_word);
    if (ret == -1) {
        perror("Error: SPI_IOC_WR_BITS fault.\n");
        return -1;
    }

    ret = ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &this->spi_freq);
    if (ret == -1) {
        perror("Error: SPI_IOC_WR_MAX_SPEED fault.\n");
        return -1;
    }

    struct spi_ioc_transfer spi { };
    spi.tx_buf = (unsigned long)&dev;
    spi.rx_buf = (unsigned long)rx_buf;
    spi.len = 1;
    spi.delay_usecs = this->delay;
    spi.speed_hz = this->spi_freq;
    spi.bits_per_word = this->bits_per_word;
    // Send WR_ADDR
    ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
    if (ret < 1) {
        perror("Error: SPI_IOC_MESSAGE fault.\n");
        return -1;
    }
    else {
        // printf("recv addr recv: 0x%x\n", rx_buf[0]);
    }

    if(rx_buf[0] != 0x00)
    {
        return -1;
    }
    tx_buf[0] = 0x01;
    spi.tx_buf = (unsigned long)tx_buf;
    spi.rx_buf = (unsigned long)id;
    spi.len = 2;
    // receive CANID
    ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
    if (ret < 1) 
    {
        perror("Error");
        return -1;
    }
    else
    {
        // printf("recv id_rcv: 0x%x\n", *id);
    }
    spi.tx_buf = (unsigned long)tx_buf;
    spi.rx_buf = (unsigned long)length;
    spi.len = 1;
    // receive length
    ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
    if (ret < 1)
    {
        perror("Error");
        return -1;
    }
    else
    {
        // printf("recv len_recv: 0x%x\n", *length);
    }

    spi.tx_buf = (unsigned long)tx_buf;
    spi.rx_buf = (unsigned long)pdata;
    spi.len = *length;
    if(spi.len != 0)
    {
        // Read Message
        ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
        if (ret < 1)
        {  
            perror("Error");
        }
        else
        {
            printf("msg: ");
            for(uint8_t i = 0; i < *length; i++)
            {
                    printf("0x%02x,", pdata[i]);
            }
            printf("\b\n");
        }  
    }
    else
    {
	    // printf("No data\n");
        ret = -1;
    }
    close(spi_fd);
    return ret;
}

int Mjbot_SPI_Interface::mjbots_control(uint8_t dev, uint16_t id, float pos, float vel, float torque, float kp, float kd)
{
    int ret = 0;
    if(kp > 1)
    {
        kp = 1;
    }else if(kp < -1)
    {
        kp = -1;
    }

    if(kd > 1)
    {
        kd = 1;
    }else if(kp < -1)
    {
        kd = -1;
    }

    
    // uint8_t cmd[] = {0x01, 0x00, 0x0A, 0x0B, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x04, 0x00};
    uint8_t cmd[] = {0x01, 0x00, 0x0A, 0x08, 0x05, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x04, 0x00, 0x50, 0x50, 0x50};
    // uint8_t cmd[] = {0x01, 0x00, 0x0A, 0x0B, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x23, 0x00, 0x00, 0x00, 0x00, 0x18, 0x04, 0x00, 0x50, 0x50, 0x50, 0x50};
    *(int32_t*)&cmd[6] = pos * 10000;
    *(int32_t*)&cmd[10] = vel * 10000;
    *(int32_t*)&cmd[14] = torque * 1000;
    *(int32_t*)&cmd[18] = kp * (2147483647) - 1;
    *(int32_t*)&cmd[22] = kd * (2147483647) - 1;
    if(0 != this->send(dev, id, cmd, sizeof(cmd)))
    {
        ret = -1;
    }
    return ret;
}
int Mjbot_SPI_Interface::mjbots_feedback(uint8_t dev, uint16_t *id, float *pos, float *vel, float *torque)
{
    int ret = 0;
    uint8_t recv[64];
    uint8_t length;
    if(this->receive(dev, id, recv, &length) != -1)
    {
        // printf("recv_suc!\n");
        if(recv[0] == 0x28 && recv[1] == 0x04 && recv[2] == 0x00)
        {
            *pos = *(int32_t*)&recv[7] * 0.00001;
            *vel = *(int32_t*)&recv[11] * 0.00001;
            *torque = *(int32_t*)&recv[15] * 0.001;
        }
        else
        {
            ret = -1;
        }
    }
    else
    {
        ret = -1;
    }
    return ret;
}